
#ifndef AFTERMATH_NUMERICS_DSSE_MATH_HPP_INCLUDED
#define AFTERMATH_NUMERICS_DSSE_MATH_HPP_INCLUDED

#include "inline.hpp"

#include <emmintrin.h>
#include <mmintrin.h>
#include <smmintrin.h>

namespace aftermath
{
    namespace numerics
    {
        OP_INLINE __m128d operator +(const __m128d& left, const __m128d& right) { return _mm_add_pd(left, right); }
        OP_INLINE __m128d operator -(const __m128d& left, const __m128d& right) { return _mm_sub_pd(left, right); }
        OP_INLINE __m128d operator *(const __m128d& left, const __m128d& right) { return _mm_mul_pd(left, right); }
        OP_INLINE __m128d operator /(const __m128d& left, const __m128d& right) { return _mm_div_pd(left, right); }
    
        OP_INLINE __m128d& operator +=(__m128d& left, const __m128d& right) { return left = _mm_add_pd(left, right); }
        OP_INLINE __m128d& operator -=(__m128d& left, const __m128d& right) { return left = _mm_sub_pd(left, right); }
        OP_INLINE __m128d& operator *=(__m128d& left, const __m128d& right) { return left = _mm_mul_pd(left, right); }
        OP_INLINE __m128d& operator /=(__m128d& left, const __m128d& right) { return left = _mm_div_pd(left, right); }

        template <int nRows, int nColumns>
        OP_INLINE void bi_row_transpose(
            const __m128d& row0, const __m128d& row1,
            __m128d& col0, __m128d& col1, __m128d& zero) 
        {
            if (nRows == 0 || nColumns == 0) return;
        
            col0 = _mm_shuffle_pd(row0, nRows == 1 ? zero : row1, 0x0);
            if (nColumns == 2) col1 = _mm_shuffle_pd(row0, nRows == 2 ? row1 : zero, 0x3);
        }

        template <int nRows>
        OP_INLINE void bi_row_transposeSelf(
            __m128d& row0, __m128d& row1,
            __m128d& tmp, __m128d& zero)
        {
            if (nRows == 0) return;
        
            if (nRows == 1) row0 = _mm_shuffle_pd(row0, zero, 0x0);
            if (nRows == 2) 
            {
                tmp = _mm_shuffle_pd(row0, row1, 0x0);
                row1 = _mm_shuffle_pd(row0, row1, 0x3);
                row0 = tmp;
            }
        }

        template <int nRows>
        OP_INLINE void bi_coupled_row_transpose(
            __m128d& urow0, __m128d& urow1,
            __m128d& lrow0, __m128d& lrow1,
            __m128d& tmp0, __m128d& tmp1, __m128d& zero) 
        {
            if (nRows == 0) return;
        
            tmp0 = _mm_shuffle_pd(urow0, urow1, 0x0);
            if (nRows == 2) tmp1 = _mm_shuffle_pd(urow0, urow1, 0x3);

            urow0 = _mm_shuffle_pd(lrow0, nRows == 2 ? lrow1 : zero, 0x0);
            urow1 = _mm_shuffle_pd(lrow0, nRows == 2 ? lrow1 : zero, 0x3);
        
            lrow0 = tmp0;
            if (nRows == 2) lrow1 = tmp1;
        }

        template <int nRows, int nColumns, bool doIncrement>
        OP_INLINE void bi_multiply(
            const __m128d& row0, const __m128d& row1,
            const __m128d& col0, const __m128d& col1,
            __m128d& res0, __m128d& res1,
            __m128d& tmp0, __m128d& tmp1, __m128d& zero) 
        {
            if (nRows == 0 || nColumns == 0) return;

            // We don't need tmp* then!
            if (nColumns == 1)
            {
                if (doIncrement)
                {
                    res0 += _mm_dp_pd(row0, col0, 0x31);
                    if (nRows == 2) res1 += _mm_dp_pd(row1, col0, 0x31);
                }
                else
                {
                    res0 = _mm_dp_pd(row0, col0, 0x31);
                    if (nRows == 2) res1 = _mm_dp_pd(row1, col0, 0x31);
                }
            }
            else
            {
                tmp0 = _mm_dp_pd(row0, col0, 0x31);
                tmp1 = _mm_dp_pd(row0, col1, 0x32);

                if (doIncrement)
                {
                    res0 += tmp0;
                    res0 += tmp1;
                }
                else res0 = tmp0 + tmp1;

                if (nRows == 2)
                {
                    tmp0 = _mm_dp_pd(row1, col0, 0x31);
                    tmp1 = _mm_dp_pd(row1, col1, 0x32);
            
                    if (doIncrement)
                    {
                        res1 += tmp0;
                        res1 += tmp1;
                    }
                    else res1 = tmp0 + tmp1;
                }
            }
        }
    }
}

#endif // AFTERMATH_NUMERICS_DSSE_MATH_HPP_INCLUDED
